#include <iostream>
#include <cmath>

#include <Eigen/Core>
#include <Eigen/Geometry>

int main(int argc, char** argv)
{
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity( );
    Eigen::Affine3d pose = Eigen::Affine3d::Identity();
    
    rotation_matrix << 4.06369316e-04,   9.98525517e-01,   5.42828403e-02,
         			   9.99043671e-01,   1.96795132e-03,  -4.36791864e-02,
        			  -4.37216082e-02,   5.42486779e-02,  -9.97569798e-01;
    pose.rotate(rotation_matrix);
       		
	pose.translation() << -0.33322, -0.11490, 0.62675;
    
//    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles( 0, 1, 2 );  // x-y-z 顺序，即rpy
//    std::cout << "roll pitch yaw = \n" << euler_angles << std::endl;

//	Eigen::Vector3d euler_angles1 = pose.rotation().eulerAngles(0, 1, 2);
//	std::cout << "roll pitch yaw in pose = \n" << euler_angles1 << std::endl;

    std::cout << "pose: \n" << pose.matrix() << std::endl;

    Eigen::AngleAxisd aa(pose.rotation());
    std::cout << aa.angle() << ", " << aa.axis() << std::endl;

    std::cout << "rotation vector: " << aa.angle() * aa.axis() << std::endl;

    return 0;
}
